#define LOG_TAG "SolarTrackActivity"
#include <c3log.h>

#include <chrono>
#include <thread>
#include <iostream>
#include <algorithm>

#include <./SolarTrackActivity.h>

#include <HTTPClient.h>
#include <WiFi.h>
#include <stdio.h>
#include <time.h>
#include <string>
#include "./c3log.h"
#include "./screen_hal/DisplayCanvas.h"
#include "oled/ESP8266_and_ESP32_OLED_driver_for_SSD1306_displays/src/OLEDDisplayUi.h"
#include "oled/ESP8266_and_ESP32_OLED_driver_for_SSD1306_displays/src/SSD1306Wire.h"
#include "oled/oled_drv_intf.h"

#include "Custom_Config.h"
#include "./cstm_driver/power_control/PwmCtl.h"

#include <SystemServer.h>
#include <WifiManager.h>
#include <TimerActivity.h>
#include <LauncherActivity.h>

#define vol2lux(VOL) (int)(VOL * 1000) \

const int SLEEP_MS = 1000;

SolarTrackActivity * SolarTrackActivity::m_pInstance = nullptr;

float g_pwm_power_map[101] = {0};
int roll_back_loop = 0;
int gLuxIdx = 0;
int gCoarseVolIdxTarget = 0;

void storePWM(float vol = 0.0, float hist_pwm = 0.0) {
    return;
}

const int QUEUE_LEN = 10;
float convergence(float power, float vol, float pwm, float pwm_adj) {
    float convergencePWM = -1;
    static float store_power[QUEUE_LEN];
    static float store_vol[QUEUE_LEN];
    static float store_pwm[QUEUE_LEN];
    static float store_power_derivative[QUEUE_LEN];
    static int cur_idx = 0;
    static uint64_t cnt = 0;
    int pre_idx = (cur_idx -1 + QUEUE_LEN) % QUEUE_LEN;
    store_power[cur_idx] = power;
    store_vol[cur_idx] = vol;
    store_pwm[cur_idx] = pwm;

    // cal derivative
    float delta_pwm = pwm - store_pwm[pre_idx];
    if (delta_pwm < 1) {
        store_power_derivative[cur_idx] = store_power_derivative[pre_idx];
    } else {
        store_power_derivative[cur_idx] = (power - store_power[pre_idx]) / delta_pwm;
    }

    // debug and find maxpower
    char outprint[128] = {0};
    float maxpower = 0;
    int maxidx = 0;
    if (cnt > QUEUE_LEN) {
        for (int i = cur_idx; i < QUEUE_LEN + cur_idx; i++) {
            int k = i % QUEUE_LEN;
            int len = strlen(outprint);
            snprintf(outprint + len, sizeof(outprint) - len, "%.3f ", store_power_derivative[k]);
            if (maxpower < store_power[k]) {
                maxpower = store_power[k];
                maxidx = k;
            }
        }
        // LOGI("store_power_derivative: %s", outprint);
        float diff_rate = (store_power[maxidx] - store_power[cur_idx]) / store_power[cur_idx];
        if (abs(maxidx - cur_idx) > 5                           // maxidx in middle stat, convergence to it
            && diff_rate > 0.10) {                              // power big 10%
            LOGI("maxpower:%.1f power_derivative:%.3f pwm:%.1f",
                maxpower,  store_power_derivative[maxidx], store_pwm[maxidx]);
            convergencePWM = store_pwm[maxidx];
        }
    }

    cur_idx++;
    cur_idx = cur_idx % QUEUE_LEN;
    cnt++;
    return convergencePWM;
}

// pwm (0.0-100.0)
float autoPWM_Algo(float cur = 0.0, float vol = 0.0, float hist_real_pwm_adj = 0.0, float hist_pwm = 0.0,
        float history_best_pwm_ref = 0.0, bool* exceptionCB = nullptr) {
    float pwm_adj = 0;
    static float history_power = 0, history_cur = 0.0, history_vol = 0.0;

    static int64_t cnt = -1;
    cnt++;
    // 1. power
    float power = cur * vol;

    // 2. set pwm
    // TBD
    float delta_vol = vol - history_vol;
    float delta_power = power - history_power;

    if (cnt == 0 || cnt == 1) {
        pwm_adj = 5;
    } else {
        if (delta_power > 0.00001) {            // L1: power increase or decrease: [0.00001, +]
            if (hist_real_pwm_adj > 0) {            // L2: pwm increase or decrease
                if (delta_vol >= -0.1) {             // L3: vol [-0.1, +]     => +2
                    pwm_adj = 2;
                    if (roll_back_loop > 0) {   // if roll back , be carefull to adj
                        pwm_adj = 0.5;
                        roll_back_loop--;
                    }
                } else if (delta_vol > -0.2) {          // L3: vol [-0.2, -0.1]  => +1
                    pwm_adj = 1;
                } else {                                // L3: vol [-, -0.2]     => +0
                    pwm_adj = 0;
                }
            } else {                                // L2: pwm increase or decrease
                if (delta_vol >= -0.1) {                // L3: vol [-0.1, +]     => +2
                    pwm_adj = 1;   // continue 0
                } else {
                    pwm_adj = 0;
                }
            }
        } else if (delta_power < -0.00001) {    // L1: power increase or decrease: [-, -0.00001]
            if (hist_real_pwm_adj > 0) {            // L2: pwm increase or decrease
                if (delta_vol >= -0.01) {               // L3: vol [-0.1, +]    => +0
                    pwm_adj = 0;
                } else {                                // L3: vol [-, -0.1]    => -1
                    float vol_left_rate = 1 + delta_vol / vol;  // vol left rate
                    pwm_adj = -1 - hist_pwm * (1 - vol_left_rate * vol_left_rate);  // back to before rate.
                    if (hist_real_pwm_adj > -pwm_adj) {
                        LOGI("rolling back. hist_real_pwm_adj: %f pwm_adj:%f");
                        pwm_adj = -hist_real_pwm_adj;   // rolling back
                    }
                    roll_back_loop = 5;
                }
            } else if (hist_real_pwm_adj < 0) {     // L2: pwm increase or decrease
                if (delta_vol >= -0.01) {               // L3: vol [-0.1, +]    => +0
                    pwm_adj = 0;
                } else {                                // L3: vol [-, -0.1]    => -1
                    pwm_adj = -1;
                    roll_back_loop = 5;
                }
            } else {                                // L2: pwm increase or decrease = 0
                if (delta_vol >= 0.01) {                // L3: vol [0.01, +]    => +1
                    pwm_adj = 0.2;
                } else if (delta_vol >= -0.01) {         // L3: vol [-0.01, 0.01]    => +0
                    pwm_adj = 0;
                } else {                                // L3: vol [-, -0.01]    => -1
                    pwm_adj = -1;  // back to before rate.
                    roll_back_loop = 5;
                }
            }
        } else {                                // L1: power increase or decrease: [-0.00001, 0.00001]
            pwm_adj = 0;
        }
    }

    // Exception, delta vol is too hight; too reason: 1. environment change 2. pwm too high
    *exceptionCB = false;
    if (delta_vol / vol < -0.2 || vol2lux(vol) < 0.6 * gLuxIdx) {
        pwm_adj = -hist_pwm;    // reset pwm to 0
        LOGW("Vol low :%.1f or cliff fall:%.1f%", vol, delta_vol / vol * 100);
        if (exceptionCB != nullptr) {
            *exceptionCB = true;
        }
    }

    // 3.store
    history_power = power;
    history_cur = cur;
    history_vol = vol;
    return pwm_adj;
}

int SolarTrackActivity::calLuxIdx() {
    float cur = 0.0, vol = 0.0, power = 0.0;
    float real_pwm = setPwm2IVP(cur, vol, power, 0, SLEEP_MS);     // PWM = 10, wait 5s
    int luxIdx = vol2lux(vol);
    if (luxIdx < 0 || luxIdx > 100000) {
        LOGE("LuxIdx out of range!, HW error");
        if (luxIdx < 0) luxIdx = 0;
        if (luxIdx > 100000) luxIdx = 100000;
    }
    return luxIdx;
}

float SolarTrackActivity::setPwm2IVP(float &cur, float &vol, float &power, int PWM, int sleepTimeMs = 1000) {
    float real_pwm = PwmCtl::getInstance()->setDuty(PWM);
    // wait stable
    std::this_thread::sleep_for(std::chrono::milliseconds(sleepTimeMs));    // sleep 100ms
    mInaApp.getIVP(cur, vol, power);
    return real_pwm;
}

float SolarTrackActivity::initPwm100() {
    const int STEP = 20;
    // coarse Search
    float suggestPWM = 0;
    float cur = 0.0, vol = 0.0, power = 0.0;
    for (int i = 0; i <= 100; i += STEP) {
        setPwm2IVP(cur, vol, power, i, SLEEP_MS);
        g_pwm_power_map[i] = power;
        LOGI("i:%d power:%f", i, g_pwm_power_map[i]);
        // interpolation
        if (i > 0) {
            float k = (g_pwm_power_map[i] - g_pwm_power_map[ i - STEP]) / (STEP * 1.0);
            for (int j = 0; j <= STEP; j++) {
                g_pwm_power_map[i - STEP + j] = g_pwm_power_map[ i - STEP] + k * j;
            }
        }
    }
    // reset PWM to 0
    setPwm2IVP(cur, vol, power, 0, SLEEP_MS);

    // search the minimum PWM and reach the base power
    for (int i = 0; i <= 100; i += STEP) {
        if (g_pwm_power_map[i] > g_pwm_power_map[100]) {
            suggestPWM = g_pwm_power_map[i];
        }
    }

    // recording Lux
    gLuxIdx = calLuxIdx();
    gCoarseVolIdxTarget = gLuxIdx * 0.8;
    LOGI("luxIdx:%d CoarseVolIdxTarget:%d suggestPWM:%.1f", gLuxIdx, gCoarseVolIdxTarget, suggestPWM);
    return suggestPWM;
}

void SolarTrackActivity::onCreate() {
    int ret = 0;
    // 4. setup INA monitor
    ret = mInaApp.setup();
    if (0 == ret) {
        mInaAppInited = true;
        LOGI("%s mInaApp init suc\n", __func__);
    } else {
        LOGE("%s mInaApp init failed\n", __func__);
    }
    // init PWM Ctl
    // pwm config: clk, mode...

    PwmCtl *pPwmCtl = PwmCtl::getInstance();
    pPwmCtl->init(IO4);
}

void SolarTrackActivity::onDraw() {
    OLEDDisplay* tempPen = DisplayCanvas::getInstance()->getPen();
    tempPen->clear();
    tempPen->setFont(ArialMT_Plain_10);

    // init algo
    static bool firstDraw = true;
    float suggestPWM = 0;
    if (firstDraw) {
        suggestPWM = initPwm100();
        firstDraw =  false;
    }

    // draw IVP
    if (mInaAppInited) {
        float pwm_adj = 0;
        float real_pwm = 0;
        float cur = 0.0, vol = 0.0, power = 0.0;
        mInaApp.getIVP(cur, vol, power);
        char tempStr[64] = {0};
        tempPen->drawStringf(0, 40, tempStr, "%.01lfA %.01lfV %.01lfW", cur, vol, power);

        // algo adjust
        static float hist_pwm = 0.0;
        static float hist_real_pwm_adj = 0.0;
        static float history_best_pwm = 0, history_best_power = 0;
        if (power > history_best_power) {
            history_best_power = power;
            history_best_pwm = hist_pwm;
        }
        bool volException = false;
        if (suggestPWM > 0) {
            // first time, set suggestPWM
            LOGI("vol:%.3f pwr:%.3f prepare next suggestPWM:%.1f", vol, power, suggestPWM);
            real_pwm = PwmCtl::getInstance()->setDuty1000(suggestPWM * 10);     // turn to 1/1000
            real_pwm = real_pwm / 10.0;     // out: turn to 1/100
            suggestPWM = 0;
        } else if (mConvStarted <= 0) {
            if (abs(gCoarseVolIdxTarget - vol2lux(vol)) / gCoarseVolIdxTarget > 0.05) {
                pwm_adj = autoPWM_Algo(cur, vol, hist_real_pwm_adj, hist_pwm, history_best_pwm, &volException);

                // exception
                if (vol2lux(vol) < 0.6 * gLuxIdx || volException) {
                    pwm_adj = -hist_pwm * 10;    // pwm reset to 0, include pwm1000
                    LOGE("VOL:%.1f exception!", vol);
                }
                LOGI("vol:%.3f pwr:%.3f prepare pwm_adj:%.1f \tnext pwm:%.1f history_best_pwm:%.1f",
                        vol, power, pwm_adj, hist_pwm + pwm_adj, history_best_pwm);
                real_pwm = PwmCtl::getInstance()->setDuty(hist_pwm + pwm_adj);
            } else {
                pwm_adj = autoPWM_Algo(cur, vol, hist_real_pwm_adj * 10, hist_pwm * 10,
                    history_best_pwm * 10, &volException);
                if (volException) pwm_adj = -hist_pwm * 10;
                pwm_adj = pwm_adj / 10;
                LOGI("vol:%.3f pwr:%.3f hist_pwm:%.1f prepare pwm_adj:%.1f \tnext pwm1000:%.1f history_best_pwm:%.1f",
                        vol, power, hist_pwm, pwm_adj, hist_pwm + pwm_adj, history_best_pwm);
                real_pwm = PwmCtl::getInstance()->setDuty1000((hist_pwm + pwm_adj) * 10);     // turn to 1/1000
                real_pwm = real_pwm / 10.0;     // out: turn to %
            }
            // try convergence detect
            float pwmConv = convergence(power, vol, real_pwm, real_pwm - hist_pwm);
            if (pwmConv > 0) {
                mConvStarted = 3;
                mConvPWM = pwmConv - 0.5;   // reduce, for good convergence
            }
        } else {    // begin tey conv, waiting 3 times
            mConvStarted--;
            LOGI("vol:%.3f pwr:%.3f hist_pwm:%.1f prepare next mConvPWM:%.1f", vol, power, hist_pwm, mConvPWM);
            real_pwm = PwmCtl::getInstance()->setDuty1000(mConvPWM * 10);     // turn to 1/1000
            real_pwm = real_pwm / 10.0;     // out: turn to %
        }

        hist_real_pwm_adj = real_pwm - hist_pwm;
        hist_pwm = real_pwm;
    }
    tempPen->display();
}

void SolarTrackActivity::onEvent() {
    LOGI("%s", __func__);
    SystemServer::getInstance()->startActivity(LauncherActivity::getInstance());
}
